﻿# coding=utf-8
# 2024/2/26: 首个版本

import bi_common as bi
from enum import Enum


class LongitudinalControlMode(Enum):
    NO_REQUEST = 0  # Not requesting longitudinal control / 不请求纵向控制
    POSITION = 1  # Control throttle and brake pedal, value ranges from -1(max brake) ~ 1(max throttle) / 控制油门、刹车踏板，数值范围为-1(刹车到底)~1(油门到底)
    ACCELERATION = 2  # Control acceleration and deceleration, in m/s² units (The target absolute speed is necessary to set) / 控制加、减速度，数值单位为m/s²，同时需要指定目标绝对速度


class LateralControlMode(Enum):
    NO_REQUEST = 0  # Not requesting lateral control / 不请求横向控制
    POSITION = 1  # Control steer, value ranges from -1(max right turning) ~ 1(max left turning) / 控制方向盘，数值范围为-1(右打到底)~1(左打到底)
    CURVATURE = 2  # Control turning curvature, in 1/m units / 控制转弯曲率，数值单位为1/m


class GearControlMode(Enum):
    NO_REQUEST = 0  # Not requesting gear switching / 不请求换挡
    DRIVE = 1  # Switch to drive gear / 请求换D挡
    REVERSE = 2  # Switch to reverse gear / 请求换R挡


class VehicleControl:
    def __init__(self):
        self.time = 0.0  # [s] Time offset in session / Session内的相对时间
        self.longitudinal_mode = LongitudinalControlMode.NO_REQUEST  # 纵向控制模式
        self.longitudinal_value = 0.0  # 纵向控制数值
        self.target_speed = 0.0  # [KPH] Target absolute speed, only available on acceleration control mode / 目标绝对速度，仅限加速度控制模式下有效
        self.lateral_mode = LateralControlMode.NO_REQUEST  # 横向控制模式
        self.lateral_value = 0.0  # 横向控制数值
        self.gear_mode = GearControlMode.NO_REQUEST  # 换挡控制模式

    # Convert to general sample for output / 转通用样本，用于样本输出
    def to_general_sample(self, channel):
        output = bi.agency.create_general_sample()
        output.protocol = 'vehicle-control-v1@' + str(channel)
        output.time = self.time
        output.significant = 6
        output.values = []
        output.values.append(self.longitudinal_mode.value)
        output.values.append(self.longitudinal_value)
        output.values.append(self.target_speed)
        output.values.append(self.lateral_mode.value)
        output.values.append(self.lateral_value)
        output.values.append(self.gear_mode.value)
        return output
